

#include "../inc/deepsort.h"
#include <memory>
#include "lyn_plugin_dev.h"
#include "trackerMgr.h"

template <typename T>
constexpr const T& clamp(const T& value, const T& lower, const T& upper) {
  return (value < lower) ? lower : ((upper < value) ? upper : value);
}

// static auto mytracker = std::make_shared<tracker>(0.2, 100);
static TrackerMgr mytrackerMgr;

constexpr uint32_t MAX_BATCH = 20;

static float half2float(int16_t ib) {
  const int sig[2] = {1, -1};
  const float result = 5.96046e-08;

  int16_t s, e, m;
  s = (ib >> 15) & 0x1;
  e = (ib >> 10) & 0x1f;
  m = ib & 0x3ff;

  // added by puyang.wang@lynxi.com
  {
    if (0 == e)
      return sig[s] * m * result;
    else {
      union {
        unsigned int u32;
        float f32;
      } ou;

      e = (0x1f == e) ? 0xff : (e - 15 + 127);
      ou.u32 = (s << 31) | (e << 23) | (m << 13);
      return ou.f32;
    }
  }
}

void lynCreateTracker(void* args)
{
  auto id = reinterpret_cast<int64_t*>(args);
  mytrackerMgr.CreateTracker(*id);
}

void lynDestroyTracker(void* args)
{
  auto id = reinterpret_cast<int64_t*>(args);
  mytrackerMgr.DestroyTracker(*id);
}

void lynDeepsort(void* args) {
  auto deepsortArg = static_cast<DeepsortArg*>(args);
  auto apuBuffer = lynPluginGetVirtAddr(deepsortArg->apuBuffer);
  auto boxInfo = (lynBoxesInfo*)lynPluginGetVirtAddr(deepsortArg->BoxInfo);

  auto boxsCount = std::min((uint32_t)boxInfo->boxesNum, MAX_BATCH);
  
  DETECTIONS detections(boxsCount);
  int16_t* int16Ptr = reinterpret_cast<int16_t*>(apuBuffer);
  for (size_t i = 0; i < boxsCount; ++i) {
    DETECTION_ROW row;
    detections[i].tlwh[0] = boxInfo->boxes[i].xmin;
    detections[i].tlwh[1] = boxInfo->boxes[i].ymin;
    detections[i].tlwh[2] = boxInfo->boxes[i].xmax - boxInfo->boxes[i].xmin;
    detections[i].tlwh[3] = boxInfo->boxes[i].ymax - boxInfo->boxes[i].ymin;
    for (int j = 0; j < 512; ++j) {
      detections[i].feature[j] = half2float(int16Ptr[i + j]);
    }
  }

  lynBoxesInfo* targets =
      (lynBoxesInfo*)lynPluginGetVirtAddr(deepsortArg->targetBoxInfo);
  targets->boxesNum = 0;

  // delete int16Ptr;
  auto mytracker = mytrackerMgr.GetTracker(deepsortArg->trackerID);
  if (mytracker.get())
  {
    mytracker->predict();
    mytracker->update(detections);

    for (size_t i = 0; i < mytracker->tracks.size() && targets->boxesNum < 128; ++i) {
    Track& track = mytracker->tracks[i];

      if (track.is_confirmed() && track.time_since_update <= 1) {
        
      auto box = track.to_tlwh();
      lynBox& target = targets->boxes[targets->boxesNum];
        target.xmin = clamp((int)box[0], 0, (int)deepsortArg->width - 1);
        target.ymin = clamp((int)box[1], 0, (int)deepsortArg->height - 1);
        target.xmax = clamp((int)(box[2] + target.xmin), 0, (int)(deepsortArg->width - 1));
        target.ymax = clamp((int)(box[3] + target.ymin), 0, (int)(deepsortArg->height - 1));
        std::string label = std::to_string(track.track_id);
        strcpy(target.label, label.c_str());

        ++targets->boxesNum;
      }
    }
  }
};